/**
 * @file ebex_gyro_sc.h
 *
 * @date Sep 27, 2011
 * @author seth
 *
 * @brief This file is part of EBEX State Estimator, created for the EBEX project
 *
 * This software is copyright (C) 2011 Columbia University
 *
 * EBEX State Estimator is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * EBEX State Estimator is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with EBEX State Estimator; if not, write to the Free Software Foundation, Inc.,
 * 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */

#ifndef EBEX_GYRO_SC_H_
#define EBEX_GYRO_SC_H_

#include <stdbool.h>
#include <stddef.h>

#include <ebex_quaternion.h>
#include <ebex_vector.h>
#include <data_file.h>
#include <ebex_genetic.h>

/// 2 star camera readings with combined variance of 5" each in RA/DEC and 20" in ROLL, expressed in radians
#define SC_CONST_VARIANCE_5_5_20 2.1153873324351638e-08

extern const size_t num_gyro_params;
/// Forward declaration from ebex_model.h
typedef struct ebex_model_s ebex_model_t;

#define _gyro_sc_parent_outputs(x,_)	\
	_(x, parent_AZ)						\
	_(x, parent_EL)						\
	_(x, parent_ROLL)

#define _gyro_sc_attitude_outputs(x,_)	\
	_(x, yaw)							\
	_(x, pitch)							\
	_(x, roll)							\
	_(x, q1)							\
	_(x, q2)							\
	_(x, q3)							\
	_(x, q4)

#define _gyro_sc_param_outputs(x,_)		\
	_(x, gy1_beta)						\
	_(x, gy2_beta)						\
	_(x, gy3_beta)						\
	_(x, gy1_symmetric_scaling)			\
	_(x, gy2_symmetric_scaling)			\
	_(x, gy3_symmetric_scaling)			\
	_(x, gy1_asymmetric_scaling)		\
	_(x, gy2_asymmetric_scaling)		\
	_(x, gy3_asymmetric_scaling)		\
	_(x, gy1_bias)						\
	_(x, gy2_bias)						\
	_(x, gy3_bias)						\
	_(x, gy1_misalignment)				\
	_(x, gy2_misalignment)				\
	_(x, gy3_misalignment)				\
	_(x, sc_yaw)						\
	_(x, sc_pitch)						\
	_(x, sc_roll)

#define _gyro_sc_uncertainty_outputs(x,_)		\
	_(x, C_xx)						\
	_(x, C_xy)						\
	_(x, C_xz)						\
	_(x, C_yx)						\
	_(x, C_yy)						\
	_(x, C_yz)						\
	_(x, C_zx)						\
	_(x, C_zy)						\
	_(x, C_zz)

EBEX_LOOKUP_TABLE(gyro_sc_parent_output, static);
EBEX_LOOKUP_TABLE(gyro_sc_attitude_output, static);
EBEX_LOOKUP_TABLE(gyro_sc_param_output, static);
EBEX_LOOKUP_TABLE(gyro_sc_uncertainty_output, static);

typedef struct
{
	double		time;
	ebex_quat_t attitude;
	ebex_quat_t uncertainty;
	size_t		sc_index;
} ebex_sc_reading_t;

typedef struct
{
	size_t		num_obs;
	size_t		gyrobox_index;
	double		*time;
	ebex_3vec_t *observation;
	ebex_3vec_t *parent;
} ebex_gyro_reading_t;

typedef struct ebex_gyro_sc_s ebex_gyro_sc_t;
typedef struct ebex_gyro_sc_s
{
	data_source_t		**gyrobox_source;
	data_source_t		**sc_source;
	data_source_t		*parent_source;

	size_t				num_gyrobox;
	size_t				num_sc;

	size_t				num_segments;
	size_t				used_segments;
	ebex_sc_reading_t 	*sc_segment;
	ebex_gyro_reading_t **gyro_segment;

	size_t				(*consume)(ebex_gyro_sc_t *m_this, size_t m_segments);
	size_t				(*produce)(ebex_gyro_sc_t *m_this, size_t m_segments);
	bool				(*reset)(ebex_gyro_sc_t *m_this);
} ebex_gyro_sc_t;


ebex_gyro_sc_t *ebex_gyro_sc_new(void);
bool ebex_gyro_sc_add_parent_src(ebex_gyro_sc_t *m_group, data_source_t *m_parent);
bool ebex_gyro_sc_add_gyrobox_src(ebex_gyro_sc_t *m_group, data_source_t *m_gyrobox);
bool ebex_gyro_sc_add_sc_src(ebex_gyro_sc_t *m_group, data_source_t *m_sc);
bool ebex_gyro_sc_organism_verify(population_t *m_pop, organism_t *m_entity);
bool ebex_gyro_sc_generate_chromosome(population_t *m_pop, organism_t *m_entity);

bool ebex_gyro_sc_register_parent_output(ebex_model_t *m_model, data_source_t **m_output, const char *m_filename, const data_file_types_t m_type);
bool ebex_gyro_sc_register_uncertainty_output(data_source_t **m_output, const char *m_filename, const data_file_types_t m_type);
bool ebex_gyro_sc_register_param_output(data_source_t **m_output, const char *m_filename, const data_file_types_t m_type);
bool ebex_gyro_sc_register_attitude_output(ebex_model_t *m_model, data_source_t **m_output, const char *m_filename, const data_file_types_t m_type);

size_t ebex_gyro_sc_output_time(data_source_t *m_file, double *m_time, size_t m_num);
size_t ebex_gyro_sc_output_attitude(data_source_t *m_file, ebex_quat_t *m_quat, const ebex_3vec_t *m_ref, const ebex_quat_t *m_rotation, size_t m_num);
size_t ebex_gyro_sc_output_params(data_source_t *m_file, gyrobox_t *m_gyrobox, sc_param_t *sc_param, const ebex_3vec_t *m_ref, size_t m_num);
size_t ebex_gyro_sc_output_uncertainties(data_source_t *m_file, ebex_mat3x3_t *m_cov, size_t m_num);
size_t ebex_gyro_sc_output_parent(data_source_t *m_file, const ebex_3vec_t *m_data, size_t m_num);

size_t ebex_gyro_sc_quat_propagate_forward(ebex_model_t *m_model, ebex_gyro_reading_t *m_gyrodata,
											 ebex_quat_t *m_output_attitude, gyrobox_t *m_gyrobox,
											 ebex_quat_t *m_intermediate_quat, size_t m_intermediate_step_size,
											 ebex_3vec_t *m_offset,
											 ebex_3vec_t *m_ratenoise, size_t m_segment);
void ebex_gyro_sc_quat_propagate_backward(ebex_model_t *model, ebex_gyro_reading_t *m_gyrodata,
											 ebex_quat_t *m_output_attitude, gyrobox_t *m_gyrobox,
											 ebex_quat_t *m_intermediate_quat, size_t m_intermediate_step_size,
											 ebex_3vec_t *m_offset, ebex_3vec_t *m_ratenoise,
											 size_t m_segment, size_t m_numsteps);
void ebex_state_print_stats(population_t *m_pop);
double ebex_gyro_sc_get_probability(ebex_quat_t *m_sc, ebex_quat_t *m_gyro, gyrobox_t *m_gyrobox, size_t m_samples, double m_gyrorate, double m_scvariance);

#endif /* EBEX_GYRO_SC_H_ */
